//////////////////////////////////////////////////////////////////////////////////////////////////////
//								Chapter 11. Eigensystems											//
//////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////
//	11.1 Jacobi Transformations of a Symmetric Matrix												//
//////////////////////////////////////////////////////////////////////////////////////////////////////
#include <math.h>
//#include "nrutil.h"

#include "eigen.h"
#ifdef _MSC_VER
#pragma warning( disable : 4244 4305)
#endif

#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
	a[k][l]=h+s*(g-h*tau);

int jacobi(double **a, int n, double d[], double **v)
//Computes all eigenvalues and eigenvectors of a real symmetric matrix a[0..n-1][0..n-1]. On
//output, elements of a above the diagonal are destroyed. d[0..n-1] returns the eigenvalues of a.
//v[0..n-1][0..n-1] is a matrix whose columns contain, on output, the normalized eigenvectors of
//a. nrot returns the number of Jacobi rotations that were required.
{
	int j,iq,ip,i;
	double tresh,theta,tau,t,sm,s,h,g,c,*b,*z;

	b=new double[n];
	z=new double[n];
	for (ip=0;ip<n;ip++) {						//Initialize to the identity matrix.
		for (iq=0;iq<n;iq++) v[ip][iq]=0.0;
		v[ip][ip]=1.0;
	}
	for (ip=0;ip<n;ip++) {						//Initialize b and d to the diagonal
												//of a. 
		b[ip]=d[ip]=a[ip][ip];
		z[ip]=0.0;									//This vector will accumulate terms
													//of the form tapq as in equa-
													//tion (11.1.14).
	}
	int nrot;
	for (i=1;i<=100;i++) {
		sm=0.0;
		for (ip=0;ip<n-1;ip++) {						//Sum o-diagonal elements.
			for (iq=ip+1;iq<n;iq++)
			sm += fabs(a[ip][iq]);
		}
		if (sm == 0.0) { //The normal return, which relies on quadratic convergence to machine under ow.
			//eigsrt(d,v,n);
			delete[] b;
			delete[] z;
			return nrot;
		}
		if (i < 4)
			tresh=0.2*sm/(n*n);								// ...on the rst three sweeps.
		else
			tresh=0.0;										//...thereafter.
		for (ip=0;ip<n-1;ip++) {
			for (iq=ip+1;iq<n;iq++) {
				g=100.0*fabs(a[ip][iq]);
				//After four sweeps, skip the rotation if the o-diagonal element is small.
				if (i > 4 && (double)(fabs(d[ip])+g) == (double)fabs(d[ip])
					&& (double)(fabs(d[iq])+g) == (double)fabs(d[iq]))
					a[ip][iq]=0.0;
				else if (fabs(a[ip][iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((double)(fabs(h)+g) == (double)fabs(h))
						t=(a[ip][iq])/h;				//t = 1=(2.)
					else {
						theta=0.5*h/(a[ip][iq]);		//Equation (11.1.10).
						t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip][iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip][iq]=0.0;
					for (j=0;j<ip-1;j++) {					//Case of rotations 1 . j < p.
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<iq-1;j++) {					//Case of rotations p < j < q.
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<n;j++) {						//Case of rotations q < j . n.
						ROTATE(a,ip,j,iq,j)
					}
					for (j=0;j<n;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++nrot;
				}
			}
		}
		for (ip=0;ip<n;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];								//Update d with the sum of tapq,
			z[ip]=0.0;									//and reinitialize z.
		}
	}
	return nrot;
	//nrerror("Too many iterations in routine jacobi");
}

int jacobi33(double a[][3],double d[], double v[][3])
//Computes all eigenvalues and eigenvectors of a real symmetric matrix a[0..n-1][0..n-1]. On
//output, elements of a above the diagonal are destroyed. d[0..n-1] returns the eigenvalues of a.
//v[0..n-1][0..n-1] is a matrix whose columns contain, on output, the normalized eigenvectors of
//a. nrot returns the number of Jacobi rotations that were required.
{
	int j,iq,ip,i;
	double tresh,theta,tau,t,sm,s,h,g,c,*b,*z;
	int n=3;

	b=new double[n];
	z=new double[n];
	for (ip=0;ip<n;ip++) {						//Initialize to the identity matrix.
		for (iq=0;iq<n;iq++) v[ip][iq]=0.0;
		v[ip][ip]=1.0;
	}
	for (ip=0;ip<n;ip++) {						//Initialize b and d to the diagonal
												//of a. 
		b[ip]=d[ip]=a[ip][ip];
		z[ip]=0.0;									//This vector will accumulate terms
													//of the form tapq as in equa-
													//tion (11.1.14).
	}
	int nrot=0;
	for (i=1;i<=100;i++) {
		sm=0.0;
		for (ip=0;ip<n-1;ip++) {						//Sum o-diagonal elements.
			for (iq=ip+1;iq<n;iq++)
			sm += fabs(a[ip][iq]);
		}
		if (sm == 0.0) { //The normal return, which relies on quadratic convergence to machine under ow.
			//eigsrt(d,v,n);
			delete[] b;
			delete[] z;
			return nrot;
		}
		if (i < 4)
			tresh=0.2*sm/(n*n);								// ...on the rst three sweeps.
		else
			tresh=0.0;										//...thereafter.
		for (ip=0;ip<n-1;ip++) {
			for (iq=ip+1;iq<n;iq++) {
				g=100.0*fabs(a[ip][iq]);
				//After four sweeps, skip the rotation if the o-diagonal element is small.
				if (i > 4 && (double)(fabs(d[ip])+g) == (double)fabs(d[ip])
					&& (double)(fabs(d[iq])+g) == (double)fabs(d[iq]))
					a[ip][iq]=0.0;
				else if (fabs(a[ip][iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((double)(fabs(h)+g) == (double)fabs(h))
						t=(a[ip][iq])/h;				//t = 1=(2.)
					else {
						theta=0.5*h/(a[ip][iq]);		//Equation (11.1.10).
						t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip][iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip][iq]=0.0;
					for (j=0;j<ip-1;j++) {					//Case of rotations 1 . j < p.
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<iq-1;j++) {					//Case of rotations p < j < q.
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<n;j++) {						//Case of rotations q < j . n.
						ROTATE(a,ip,j,iq,j)
					}
					for (j=0;j<n;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++nrot;
				}
			}
		}
		for (ip=0;ip<n;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];								//Update d with the sum of tapq,
			z[ip]=0.0;									//and reinitialize z.
		}
	}
	return nrot;
	//nrerror("Too many iterations in routine jacobi");
}

#undef ROTATE
///////////////////////////////////////////////////////////////////////////////////////////////////////
void eigsrt(double d[], double **v, int n)
//Given the eigenvalues d[1..n] and eigenvectors v[1..n][1..n] as output from jacobi
//( x 11.1) or tqli ( x 11.3), this routine sorts the eigenvalues into descending order, and rearranges
//the columns of v correspondingly. The method is straight insertion.
{
	int k,j,i;
	double p;
	for (i=0;i<n-1;i++) {
		p=d[k=i];
		for (j=i+1;j<n;j++)
		if (d[j] <= p) p=d[k=j];
		if (k != i) {
			d[k]=d[i];
			d[i]=p;
			for (j=0;j<n;j++) {
				p=v[j][i];
				v[j][i]=v[j][k];
				v[j][k]=p;
			}
		}
	}
}

void eigsrt(double d[], double v[][SIZE_MAX*DGR], int n)
//Given the eigenvalues d[1..n] and eigenvectors v[1..n][1..n] as output from jacobi
//( x 11.1) or tqli ( x 11.3), this routine sorts the eigenvalues into descending order, and rearranges
//the columns of v correspondingly. The method is straight insertion.
{
	int k,j,i;
	double p;
	for (i=0;i<n-1;i++) {
		p=d[k=i];
		for (j=i+1;j<n;j++)
		if (d[j] <= p) p=d[k=j];
		if (k != i) {
			d[k]=d[i];
			d[i]=p;
			for (j=0;j<n;j++) {
				p=v[j][i];
				v[j][i]=v[j][k];
				v[j][k]=p;
			}
		}
	}
}


//================
int eigen(double **A,int ndim_,double *eigVal,double **eigVec){
	long i,j,ndim=ndim_;
	double *temp,*vr,*eigVal_img,*work;
	long info,lwork=(ndim>0)?(ndim*6):1;
	
	temp=new double[ndim*ndim];
	vr=new double[ndim*ndim];
	eigVal_img=new double[ndim];
	work=new double[lwork];
	char jobvl='N',jobvr='V';
	
	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) temp[i*ndim+j]=A[i][j];

	dgeev_(&jobvl, &jobvr,&ndim,temp,&ndim, eigVal, eigVal_img, 0, &ndim, vr, &ndim, work, &lwork, &info);

	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) eigVec[i][j]=vr[i*ndim+j];

	delete[] temp;
	delete[] vr;
	delete[] eigVal_img;
	delete[] work;
	
	return info;
}

int eigen(double A[][SIZE_MAX*DGR],int ndim_,double *eigVal,double eigVec[][SIZE_MAX*DGR]){
	long i,j,ndim=ndim_;
	
	long info,lwork=(ndim>0)?(ndim*6):1;
	
	double temp[SIZE_MAX*DGR*SIZE_MAX*DGR],vr[SIZE_MAX*DGR*SIZE_MAX*DGR],eigVal_img[SIZE_MAX*DGR],work[SIZE_MAX*DGR*6];
	
	char jobvl='N',jobvr='V';
	
	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) temp[i*ndim+j]=A[i][j];

	dgeev_(&jobvl, &jobvr,&ndim,temp,&ndim, eigVal, eigVal_img, 0, &ndim, vr, &ndim, work, &lwork, &info);

	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) eigVec[i][j]=vr[i*ndim+j];

	
	return info;
}

int eigen33(double A[][3],double *eigVal,double eigVec[][3]){
	long i,j,ndim=3;
	double *temp,*vr,*eigVal_img,*work;
	long info,lwork=(ndim>0)?(ndim*6):1;
	
	temp=new double[ndim*ndim];
	vr=new double[ndim*ndim];
	eigVal_img=new double[ndim];
	work=new double[lwork];
	char jobvl='N',jobvr='V';
	
	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) temp[i*ndim+j]=A[i][j];

	dgeev_(&jobvl, &jobvr,&ndim,temp,&ndim, eigVal, eigVal_img, 0, &ndim, vr, &ndim, work, &lwork, &info);

	for(i=0;i<ndim;i++)
		for(j=0;j<ndim;j++) eigVec[i][j]=vr[i*ndim+j];

	delete[] temp;
	delete[] vr;
	delete[] eigVal_img;
	delete[] work;
	
	return info;
	
}

//*/
